#include "rviz/display_context.h"
#include "rviz/properties/string_property.h"
#include "rviz/properties/bool_property.h"

#include"pose_send_tool.h"

namespace path_creator_rviz_plugin
{
    // constructor
    PoseSendTool::PoseSendTool()
    {
        shortcut_key_ = 'q';

        topic_property_ = new rviz::StringProperty("Topic", "mbf_actions/pose_for_path",
                                                    "The topic where the pose is send to make it part of path",
                                                    getPropertyContainer(), SLOT( updateTopic() ), this );
    }

    // init
    void PoseSendTool::onInitialize()
    {
        PoseTool::onInitialize();
        setName( "Send pose for path" );
        updateTopic();

    }

    // The old ``clicked_pose_pub_`` is destroyed by this assignment,
    // and thus the old topic advertisement is removed.  The call to
    // nh_.advertise() says we want to publish data on the new topic
    // name.
    void PoseSendTool::updateTopic()
    {
        clicked_pose_pub_ = nh_.advertise<geometry_msgs::Pose>( topic_property_->getStdString(), 1);
    }

    
    void PoseSendTool::onPoseSet(double x, double y, double theta)
    {
        std::string fixed_frame = context_->getFixedFrame().toStdString();
        geometry_msgs::Pose pose;

        pose.position.x =  x;
        pose.position.y =  y;

        tf::Quaternion quat;    
        quat.setRPY(0.0, 0.0, theta);
        tf::quaternionTFToMsg(quat,
                                pose.orientation);
        ROS_INFO("Setting pose: %.3f %.3f %.3f [frame=%s]", x, y, theta, fixed_frame.c_str());

        clicked_pose_pub_.publish(pose);
    }
}

#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(path_creator_rviz_plugin::PoseSendTool, rviz::Tool )